/**
 * @file        mdw_pid.c
 * @brief       XXXX
 * @note        XXXX
 * @author      靳普诏(puzhao.jin@hopechart.com)
 * @date        2024/04/06
 * @version     1.0

 * @par         修改日志
 * <table>
 * <tr><th>Date         <th>Version     <th>Author      <th> Description
 * <tr><td>2024/04/06   <td>1.0         <td>靳普诏       <td> 创建初始版本
 * @copyright 杭州鸿泉物联网技术股份有限公司
 */
 
#include "mdw_pid.h"

#include <string.h>

#define INT64_MAX ((Int64)9223372036854775807LL)
#define INT64_MIN ((Int64)(-9223372036854775807LL - 1))


Int64 L_Mdw_SafeSum(Int64 a, Int64 b, Int64 min, Int64 max)
{
    // 检查是否可能发生整数溢出
    if ((b > 0) && (a > INT64_MAX - b)) {
        // 如果a和b的和大于INT64_MAX，则返回max
        return max;
    } else if ((b < 0) && (a < INT64_MIN - b)) {
        // 如果a和b的和小于INT64_MIN，则返回min
        return min;
    }
    
    Int64 sum = a + b;
    
    // 确保结果落在[min, max]的范围内
    if (sum < min) {
        return min;
    } else if (sum > max) {
        return max;
    } else {
        return sum;
    }
}

Int64 L_Mdw_SafeDiff(Int64 a, Int64 b, Int64 min, Int64 max)
{
    return L_Mdw_SafeSum(a, -b, min, max);
}

Int32 Mdw_TPidCreate(Mdw_TPid *self, Int64 kp, Int64 ki, Int64 kd, Int64 max_out, Int64 min_out)
{
    Int32 result = -1;

    if (self != NULL)
    {
        self->kp = kp;
        self->ki = ki;
        self->kd = kd;
        self->max_out = max_out;
        self->min_out = min_out;

        self->max_err = INT64_MAX;
        self->min_err = INT64_MIN;

        self->max_err_diff = INT64_MAX;
        self->min_err_diff = INT64_MIN;

        self->max_err_sum = INT64_MAX;
        self->min_err_sum = INT64_MIN;

        Mdw_TPidReset(self);

        result = 0;
    }

    return result;
}


void Mdw_TPidDestroy(Mdw_TPid *self)
{
    if (self != NULL)
    {
        memset(self, 0, sizeof(*self));
    }
}


Int32 Mdw_TPidReset(Mdw_TPid *self)
{
    Int32 result = -1;

    if (self != NULL)
    {
        self->last_err_ = 0;
        self->err_sum_ = 0;

        result = 0;
    }

    return result;
}


Int64 Mdw_TPidCalc(Mdw_TPid *self, Int64 curr_err)
{
    Int64 p_out = 0;
    Int64 i_out = 0;
    Int64 d_out = 0;
    
    curr_err = CLAMP(curr_err, self->min_err, self->max_err);

    // Kp 计算
    p_out = self->kp * curr_err;


    // Ki 计算
    self->err_sum_ = L_Mdw_SafeSum(curr_err, self->err_sum_, self->min_err_sum, self->max_err_sum);
    i_out = self->ki * self->err_sum_;

    // Kd 计算
    d_out = self->kd * L_Mdw_SafeDiff(curr_err, self->last_err_, self->min_err_diff, self->max_err_diff);
    self->last_err_ = curr_err;


    p_out = L_Mdw_SafeSum(p_out, i_out, self->min_out, self->max_out);
    p_out = L_Mdw_SafeSum(p_out, d_out, self->min_out, self->max_out);

    return p_out;
}






///< Generated on "2024-04-06 18:44:01" by the tool "gen_hq_file.py >> V20231119" 

